/**
 * @file Params.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 全局参数类
 */

#include "Params.h"

Params PARAMS; // 全局参数

/**
 * @brief 根据地图号选择参数方案
 * @param map 地图号
 */
void Params::setMap(int map) {
    this->map = map;
    switch (map)
    {
    case 1: {
        /* 运动控制 */
        FORWARD_PID_EMPTY = PIDParam_t(5); // 空载时的前进PID参数
        FORWARD_PID_FULL = PIDParam_t(3.3); // 满载时的前进PID参数
        FORWARD_FINISH_PRECISION = 0.5; // 前进PID判定控制是否完成时的控制精度
        ROTATE_PID_EMPTY = PIDParam_t(30, 8); // 空载时的旋转PID参数
        ROTATE_PID_FULL = PIDParam_t(20, 8); // 满载时的旋转PID参数
        ROTATE_FINISH_PRECISION = 0.1; // 旋转PID判定控制是否完成时的控制精度
        ROTATE_CAN_RUN_PRECISION = PI/3.5; // 旋转PID判定是否可以前进的控制精度
        WAIT_ROTATE_SPEED_EMPTY = 6; // 空载时等待旋转任务完成的最大速度
        WAIT_ROTATE_SPEED_FULL = 6; // 满载时等待旋转任务完成的最大速度

        /* 寻路 */
        LINE_COST = 1; //  直线一格消耗
        INCLINE_COST = 1.414; // 斜线一格消耗
        OBSTACLE_COST = 1.5; // 目标格周围障碍物数量消耗
        ROTATE_COST = 1.414; // 旋转PI/4消耗

        /* 避让 */
        IN_PATH_COST = 1; // 在冲突格上移动的消耗
        FORCAST_STEPS = 50; // 冲突检测的步数
        SAFETY_STEPS = 20; // 安全检测的步数
        SAFETY_DISTANCE_EMPTY = ROBOT_WIDTH_EMPTY + 1; // 空载时安全距离
        SAFETY_DISTANCE_FULL = ROBOT_WIDTH_FULL + 2; // 满载时安全距离

        /* 规划 */
        STEP_FRAMES_EMPTY = 2; // 空载走一步消耗的帧数
        STEP_FRAMES_FULL = 3; // 满载走一步消耗的帧数
        GAP_FRAMES = -2000; // 结束时不分配方案的预留帧数
        REPLAN_FRAMES = 0; // 没有方案分配时的等待帧数
        BENCH9_LOSS_RATE = 0.2; // 把非7号货物卖给9号工作台的价值损失率
        break;
    }

    case 2: {
        /* 运动控制 */
        FORWARD_PID_EMPTY = PIDParam_t(2.32, 0, 1e-1, 2); // 空载时的前进PID参数
        FORWARD_PID_FULL = PIDParam_t(2.23, 0, 1e-1, 1.5); // 满载时的前进PID参数
        FORWARD_FINISH_PRECISION = 0.3; // 前进PID判定控制是否完成时的控制精度
        ROTATE_PID_EMPTY = PIDParam_t(30, 8); // 空载时的旋转PID参数
        ROTATE_PID_FULL = PIDParam_t(20, 8); // 满载时的旋转PID参数
        ROTATE_FINISH_PRECISION = 0.1; // 旋转PID判定控制是否完成时的控制精度
        ROTATE_CAN_RUN_PRECISION = PI/3.5; // 旋转PID判定是否可以前进的控制精度
        WAIT_ROTATE_SPEED_EMPTY = 6; // 空载时等待旋转任务完成的最大速度
        WAIT_ROTATE_SPEED_FULL = 6; // 满载时等待旋转任务完成的最大速度

        /* 寻路 */
        LINE_COST = 1; //  直线一格消耗
        INCLINE_COST = 1.414; // 斜线一格消耗
        OBSTACLE_COST = 1.5; // 目标格周围障碍物数量消耗
        ROTATE_COST = 1.414; // 旋转PI/4消耗

        /* 避让 */
        IN_PATH_COST = 2; // 在冲突格上移动的消耗
        FORCAST_STEPS = 50; // 冲突检测的步数
        SAFETY_STEPS = 20; // 安全检测的步数
        SAFETY_DISTANCE_EMPTY = ROBOT_WIDTH_EMPTY; // 空载时安全距离
        SAFETY_DISTANCE_FULL = ROBOT_WIDTH_FULL; // 满载时安全距离

        /* 规划 */
        STEP_FRAMES_EMPTY = 5; // 空载走一步消耗的帧数
        STEP_FRAMES_FULL = 6; // 满载走一步消耗的帧数
        GAP_FRAMES = 0; // 结束时不分配方案的预留帧数
        REPLAN_FRAMES = 100; // 没有方案分配时的等待帧数
        BENCH9_LOSS_RATE = 0.2; // 把非7号货物卖给9号工作台的价值损失率
        break;
    }

    case 3: {
        /* 运动控制 */
        FORWARD_PID_EMPTY = PIDParam_t(2.32, 0, 1e-1, 2); // 空载时的前进PID参数
        FORWARD_PID_FULL = PIDParam_t(2.23, 0, 1e-1, 1.5); // 满载时的前进PID参数
        FORWARD_FINISH_PRECISION = 0.5; // 前进PID判定控制是否完成时的控制精度
        ROTATE_PID_EMPTY = PIDParam_t(30, 8); // 空载时的旋转PID参数
        ROTATE_PID_FULL = PIDParam_t(20, 8); // 满载时的旋转PID参数
        ROTATE_FINISH_PRECISION = 0.1; // 旋转PID判定控制是否完成时的控制精度
        ROTATE_CAN_RUN_PRECISION = PI/3.5; // 旋转PID判定是否可以前进的控制精度
        WAIT_ROTATE_SPEED_EMPTY = 6; // 空载时等待旋转任务完成的最大速度
        WAIT_ROTATE_SPEED_FULL = 6; // 满载时等待旋转任务完成的最大速度

        /* 寻路 */
        LINE_COST = 1; //  直线一格消耗
        INCLINE_COST = 1.414; // 斜线一格消耗
        OBSTACLE_COST = 1.5; // 目标格周围障碍物数量消耗
        ROTATE_COST = 1.414; // 旋转PI/4消耗

        /* 避让 */
        IN_PATH_COST = 1; // 在冲突格上移动的消耗
        FORCAST_STEPS = 30; // 冲突检测的步数
        SAFETY_STEPS = 15; // 安全检测的步数
        SAFETY_DISTANCE_EMPTY = ROBOT_WIDTH_EMPTY; // 空载时安全距离
        SAFETY_DISTANCE_FULL = ROBOT_WIDTH_FULL; // 满载时安全距离

        /* 规划 */
        STEP_FRAMES_EMPTY = 5; // 空载走一步消耗的帧数
        STEP_FRAMES_FULL = 6; // 满载走一步消耗的帧数
        GAP_FRAMES = 0; // 结束时不分配方案的预留帧数
        REPLAN_FRAMES = 0; // 没有方案分配时的等待帧数
        BENCH9_LOSS_RATE = 0; // 把非7号货物卖给9号工作台的价值损失率
        break;
    }

    case 4: {
        /* 运动控制 */
        FORWARD_PID_EMPTY = PIDParam_t(2.32, 0, 1e-1, 2); // 空载时的前进PID参数
        FORWARD_PID_FULL = PIDParam_t(2.23, 0, 1e-1, 1.5); // 满载时的前进PID参数
        FORWARD_FINISH_PRECISION = 0.3; // 前进PID判定控制是否完成时的控制精度
        ROTATE_PID_EMPTY = PIDParam_t(30, 8); // 空载时的旋转PID参数
        ROTATE_PID_FULL = PIDParam_t(20, 8); // 满载时的旋转PID参数
        ROTATE_FINISH_PRECISION = 0.1; // 旋转PID判定控制是否完成时的控制精度
        ROTATE_CAN_RUN_PRECISION = PI/3.5; // 旋转PID判定是否可以前进的控制精度
        WAIT_ROTATE_SPEED_EMPTY = 6; // 空载时等待旋转任务完成的最大速度
        WAIT_ROTATE_SPEED_FULL = 6; // 满载时等待旋转任务完成的最大速度

        /* 寻路 */
        LINE_COST = 1; //  直线一格消耗
        INCLINE_COST = 1.414; // 斜线一格消耗
        OBSTACLE_COST = 1.5; // 目标格周围障碍物数量消耗
        ROTATE_COST = 1.414; // 旋转PI/4消耗

        /* 避让 */
        IN_PATH_COST = 1; // 在冲突格上移动的消耗
        FORCAST_STEPS = 50; // 冲突检测的步数
        SAFETY_STEPS = 20; // 安全检测的步数
        SAFETY_DISTANCE_EMPTY = ROBOT_WIDTH_EMPTY; // 空载时安全距离
        SAFETY_DISTANCE_FULL = ROBOT_WIDTH_FULL; // 满载时安全距离

        /* 规划 */
        STEP_FRAMES_EMPTY = 5; // 空载走一步消耗的帧数
        STEP_FRAMES_FULL = 6; // 满载走一步消耗的帧数
        GAP_FRAMES = 0; // 结束时不分配方案的预留帧数
        REPLAN_FRAMES = 100; // 没有方案分配时的等待帧数
        BENCH9_LOSS_RATE = 0.2; // 把非7号货物卖给9号工作台的价值损失率
        break;
    }

    default:
        break;
    }
}
